from pyb import Pin, SPI, Timer, UART, millis
from time import sleep
from sensor import set_vflip,set_hmirror,reset,set_pixformat,set_framesize,alloc_extra_fb,snapshot,get_windowing
from pyb import ExtInt
from gc import collect
from math import radians,sin,cos,sqrt,floor
import lcd,text
touch_calibration_data=[1, 142, 13, 226, 38, 133, 2, 46, 12, 149, 43, 85, 5, 94, 12, 85, 15, 35, 8, 212, 23]

uart = UART(2, 115200,read_buf_len=10)
select_rect_busy=False
#手柄变量
gamepad_data=[255,255,127,128,127,128]  #手柄数据
check=[0,0,0,0,0,0,0,0,0,0,0]   #校验数据
gamepad_online=0    #手柄在线状态
gamepad_online_timeout=0    #手柄在线状态记录超时
gamepad_receive=0   #是否有接收到辅助控制器的手柄数据
#系统状态、功能变量
charging_state=2    #充电状态：1开启，0关闭，2没有接收到
vout_state=2    #5V输出状态：1开启，0关闭，2没有接收到
servo1_state=2  #三路舵机通道的开关状态
servo2_state=2
servo3_state=2
angle1=None
angle2=None
angle3=None
sales_date=[0,0,0]
battery1=0
battery2=0
battery_reference=0
battery_threshold=0
battery_difference_threshold=0
#从51获取的数据
XX=0
YY=0
ZZ=0
#需要保存的屏幕校准值
x_min=0
x_max=0
x_stepping=0
y_min=0
y_max=0
y_stepping=0
z_min=0
z_max=0
z_x_stepping=0
z_y_stepping=0
#最终计算、判断得到的触摸坐标值
x=0
y=0
touch_calibration_check=0
warning=0#特殊警报事件，0无，1低电量关机，2压差关机
zoom_scale_w=1  #长宽缩放比例，用于缩放后处理触摸数据
zoom_scale_h=1
#与辅助控制器51通讯
def uart_r(nn):
    global gamepad_data,check,gamepad_online,gamepad_online_timeout,gamepad_receive
    global charging_state,vout_state,servo1_state,servo2_state,servo3_state
    global angle1,angle2,angle3,sales_date,battery1,battery2,battery_reference
    global battery_threshold,battery_difference_threshold
    global XX,YY,ZZ
    global x_min,x_max,x_stepping,y_min,y_max,y_stepping
    global z_min,z_max,z_x_stepping,z_y_stepping,touch_calibration_check
    global warning
    check[0]=uart.readchar()
#1:手柄已连接
    if check[0]==1:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        check[2]=uart.readchar()
        check[3]=uart.readchar()
        check[4]=uart.readchar()
        check[5]=uart.readchar()
        check[6]=uart.readchar()
        if check[6]==(check[0]^check[1]^check[2]^check[3]^check[4]^check[5]):
            gamepad_data[0]=check[0]
            gamepad_data[1]=check[1]
            gamepad_data[2]=check[2]
            gamepad_data[3]=check[3]
            gamepad_data[4]=check[4]
            gamepad_data[5]=check[5]
            gamepad_online=1
            gamepad_online_timeout=0
            gamepad_receive=1
        else:
            gamepad_online_timeout+=1
            if gamepad_online_timeout>10:#连续10次校验错误即认为没有连接手柄
                gamepad_online=0
                gamepad_receive=0
#2:手柄未连接
    elif check[0]==2:
        if uart.readchar()==0xA1:
            gamepad_online=0
            gamepad_receive=1

#15:触摸屏坐标获取
    elif check[0]==15:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        check[2]=uart.readchar()
        check[3]=uart.readchar()
        check[4]=uart.readchar()
        check[5]=uart.readchar()
        check[6]=uart.readchar()
        if 15^check[0]^check[1]^check[2]^check[3]^check[4]^check[5]==check[6]:
            XX=int(check[0]*256+check[1])
            YY=int(check[2]*256+check[3])
            ZZ=int(check[4]*256+check[5])
#3:是否开启充电
    elif check[0]==3:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        if check[1]+check[0]==0xff:
            charging_state=check[0]
#4:是否开启5V输出
    elif check[0]==4:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        if check[1]+check[0]==0xff:
            vout_state=check[0]
#5:是否开启舵机通道1
    elif check[0]==5:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        if check[1]+check[0]==0xff:
            servo1_state=check[0]
#6:是否开启舵机通道2
    elif check[0]==6:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        if check[1]+check[0]==0xff:
            servo2_state=check[0]
#7:是否开启舵机通道3
    elif check[0]==7:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        if check[1]+check[0]==0xff:
            servo3_state=check[0]
#8:舵机通道1数值
    elif check[0]==8:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        check[2]=uart.readchar()
        if 8^check[0]^check[1]==check[2]:
            angle1=check[0]*256+check[1]
#9:舵机通道2数值
    elif check[0]==9:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        check[2]=uart.readchar()
        if 9^check[0]^check[1]==check[2]:
            angle2=check[0]*256+check[1]
#10:舵机通道3数值
    elif check[0]==10:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        check[2]=uart.readchar()
        if 10^check[0]^check[1]==check[2]:
            angle3=check[0]*256+check[1]
#11:销售日期
    elif check[0]==11:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        check[2]=uart.readchar()
        check[3]=uart.readchar()
        if 11^check[0]^check[1]^check[2]==check[3]:
            sales_date[0]=check[0]
            sales_date[1]=check[1]
            sales_date[2]=check[2]
#12:电池电压
    elif check[0]==12:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        check[2]=uart.readchar()
        check[3]=uart.readchar()
        check[4]=uart.readchar()
        check[5]=uart.readchar()
        check[6]=uart.readchar()
        if 12^check[0]^check[1]^check[2]^check[3]^check[4]^check[5]==check[6]:
            if check[0]&0x80>0:
                check[0]-=0x80
                print('电量低！系统即将自动关机！')
                warning=1
            if check[2]&0x80>0:
                check[2]-=0x80;
                print('两节电池电量差异过大，即将自动关机！请充满电以修复。如果频繁出现此提示，尝试修改电量差警报阈值，或更换电池。')
                warning=2
            battery1 = check[0]*256+check[1]
            battery2 = check[2]*256+check[3]
            battery_reference = check[4]*256+check[5]
#13:电池低电量关机阈值
    elif check[0]==13:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        check[2]=uart.readchar()
        if 13^check[0]^check[1]==check[2]:
            battery_threshold=check[0]*256+check[1]
#14:电池压差关机阈值
    elif check[0]==14:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        check[2]=uart.readchar()
        if 14^check[0]^check[1]==check[2]:
            battery_difference_threshold=check[0]*256+check[1]
#16:获取触屏校准x值
    elif check[0]==16:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        check[2]=uart.readchar()
        check[3]=uart.readchar()
        check[4]=uart.readchar()
        check[5]=uart.readchar()
        check[6]=uart.readchar()
        if 16^check[0]^check[1]^check[2]^check[3]^check[4]^check[5]==check[6]:
            x_min=check[0]*256+check[1]
            x_max=check[2]*256+check[3]
            x_stepping=check[4]*256+check[5]
#17:获取触屏校准y值
    elif check[0]==17:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        check[2]=uart.readchar()
        check[3]=uart.readchar()
        check[4]=uart.readchar()
        check[5]=uart.readchar()
        check[6]=uart.readchar()
        if 17^check[0]^check[1]^check[2]^check[3]^check[4]^check[5]==check[6]:
            y_min=check[0]*256+check[1]
            y_max=check[2]*256+check[3]
            y_stepping=check[4]*256+check[5]
#18:获取触屏校准z值
    elif check[0]==18:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        check[2]=uart.readchar()
        check[3]=uart.readchar()
        check[4]=uart.readchar()
        check[5]=uart.readchar()
        check[6]=uart.readchar()
        check[7]=uart.readchar()
        check[8]=uart.readchar()
        if 18^check[0]^check[1]^check[2]^check[3]^check[4]^check[5]^check[6]^check[7]==check[8]:
            z_min=check[0]*256+check[1]
            z_max=check[2]*256+check[3]
            z_x_stepping=check[4]*256+check[5]
            z_y_stepping=check[6]*256+check[7]
#19:屏幕校准数据
    elif check[0]==19:
        check[0]=uart.readchar()
        check[1]=uart.readchar()
        if 19^check[0]==check[1]:
            touch_calibration_check=check[0]
#清空缓存区
    while uart.any()>0:
        check[0]=uart.readchar()

extint4 = ExtInt('PC4',287375360,1,uart_r)#下降沿触发，上拉

#对副主控制器51的通用发送程序
def uart_send_function(n):
    uart.writechar(0x47)
    uart.writechar(0x65)
    uart.writechar(n)
    uart.writechar(255-n)

def uart_send_char(n,charn):
    uart.writechar(0x47)
    uart.writechar(0x65)
    uart.writechar(n)
    uart.writechar(charn)
    uart.writechar(n^charn)

def uart_send_int(n,intn):
    uart.writechar(0x47)
    uart.writechar(0x65)
    uart.writechar(n)
    uart.writechar(int(intn/256))
    uart.writechar(int(intn%256))
    uart.writechar(n^int(intn/256)^int(intn%256))

battery_display_state=False
battery_x=260
battery_y=21
battery_erase=False
battery_color=(255,255,255)
battery_background=(0,0,0)
battery_mode=0
display_high_speed=0
def draw_battery(img):
    global battery_x,battery_y,battery_color,battery_background,battery_mode
    if battery_erase:
        img.draw_rectangle(battery_x,battery_y,40,15,color=battery_background,fill=True) #电池框
    img.draw_rectangle(battery_x,battery_y,36,15,color=battery_color) #电池框
    img.draw_rectangle(battery_x+35,battery_y+3,5,9,color=battery_color)   #电池头
    if battery_mode==0:
        global battery1,battery_reference
        battery_threshold=(7,7.48,7.58,7.74,7.96)
        battery_all=battery1/4096*(1.19/(battery_reference/4096))*2.82
        for nn in range(0,4):
            if battery_all>battery_threshold[nn]:
                img.draw_rectangle(battery_x+3+8*nn,battery_y+2,6,11,color=battery_color,fill=True) #电池段
    elif battery_mode==1:
        global battery1,battery_reference
        img.draw_string(battery_x+3,battery_y+3,str(round(battery1/4096*(1.19/(battery_reference/4096))*2.82,2)),x_spacing=-1)

def display_with_battery(img):
    global battery_display_state,warning,battery_erase
    if warning==0:#正常显示状态
        if battery_display_state:
            draw_battery(img)
    elif warning==1:#低电量
        d=hardware()
        img.draw_rectangle(58,33,204,65,color=(0,0,0),fill=True)
        img.draw_rectangle(58,33,204,65,color=(255,0,0))
        text.word(img,60,35,'电量过低,即将自动关机!',color=(255,0,0))
        text.word(img,60,55,'电池电压:'+str(round(d.get_battery_all(),3))+'V',color=(120,120,120))
        text.word(img,60,75,'关机阈值:'+str(round(d.battery_threshold(),3))+'V',color=(120,120,120))
    elif warning==2:#压差
        d=hardware()
        img.draw_rectangle(62,108,196,105,color=(0,0,0),fill=True)
        img.draw_rectangle(62,108,196,105,color=(255,0,0))
        text.word(img,65,110,'电量低或电池差异过大!',color=(255,0,0))
        text.word(img,65,130,'即将自动关机!',color=(255,0,0))
        text.word(img,65,150,'电池1电压:'+str(round(d.get_battery_1(),3))+'V',color=(120,120,120))
        text.word(img,65,170,'电池2电压:'+str(round(d.get_battery_2(),3))+'V',color=(120,120,120))
        text.word(img,65,190,'压差阈值:'+str(round(d.battery_difference_threshold(),3))+'V',color=(120,120,120))

class screen:
    def __init__(self,firsttime=False,high_speed=0):
        global x_stepping,y_stepping,z_x_stepping,z_y_stepping
        self.cs=Pin('PD10', Pin.OUT_PP)
        self.rst=Pin('PD8', Pin.OUT_PP)
        self.rs =Pin('PD9', Pin.OUT_PP)
        self.backlight_pin=Pin('PD14', Pin.OUT_PP,Pin.PULL_UP)
        self.backlight_pin.high()
        self.buzzer=Pin('PD15', Pin.OUT_PP)
        self.buzzer.low()
        display_high_speed=high_speed
        reset()
        set_pixformat(3)    #sensor.RGB565
        set_framesize(10)   #sensor.QVGA
        set_vflip(True)     #翻转画面
        set_hmirror(True)
        self.high_speed_mode(high_speed)
        self.x=-1
        self.y=-1
        self.backlight_pin.high()
        #从51读取触控校准值
        if not firsttime:
            timeout=30
            x_stepping=123456
            while timeout>0 and x_stepping==123456:
                timeout-=1
                uart_send_function(24)
                sleep(0.1)
            if x_stepping==0 or x_min>60000:
                raise Exception('E20')
            else:
                x_stepping/=1000

            timeout=30
            y_stepping=123456
            while timeout>0 and y_stepping==123456:
                timeout-=1
                uart_send_function(25)
                sleep(0.1)
            if y_stepping==0 or y_min>60000:
                raise Exception('E21')
            else:
                y_stepping/=1000

            timeout=30
            z_x_stepping=123456
            while timeout>0 and z_x_stepping==123456:
                timeout-=1
                uart_send_function(26)
                sleep(0.1)
            if z_x_stepping==0 or z_min>60000:
                raise Exception('E22')
            else:
                z_x_stepping/=1000
                z_y_stepping/=1000
        self.backlight_pin.high()
        self.high_speed_mode(high_speed)

    def high_speed_mode(self,mode):
        global display_high_speed
        display_high_speed=mode
        self.spi=SPI(2,SPI.MASTER,baudrate=70000000)
        #sleep(0.01)
        lcd.init(width=320,height=240)
        self.write_c(0x36)#设置屏幕方向
        self.write_d(0xa0)
        self.set_resolution(0,0,320,240)
        lcd.clear()
        if mode==1:
            lcd.init(width=240,height=180,triple_buffer=True,refresh=60)
            self.write_c(0x36)#设置屏幕方向
            self.write_d(0xa0)
            self.set_resolution(40,30,240,180)
        elif mode==2:
            lcd.init(width=160,height=120,triple_buffer=True,refresh=120)
            self.write_c(0x36)#设置屏幕方向
            self.write_d(0xa0)
            self.set_resolution(80,60,160,120)

    def set_resolution(self,x,y,w,h):
        self.write_c(0x2a)#设置屏幕长宽像素
        self.write_d(int(x/256))
        self.write_d(x%256)
        self.write_d(int((x+w-1)/256))
        self.write_d((x+w-1)%256)
        self.write_c(0x2b)
        self.write_d(int(y/256))
        self.write_d(y%256)
        self.write_d(int((y+h-1)/256))
        self.write_d((y+h-1)%256)
        self.write_c(0x2C)

    def write_c(self,c):
        self.cs.low()
        self.rs.low()
        self.spi.send(c)
        self.cs.high()

    def write_d(self,c):
        self.cs.low()
        self.rs.high()
        self.spi.send(c)
        self.cs.high()

    def turn_on_backlight(self):
        self.backlight_pin.high()

    def turn_off_backlight(self):
        self.backlight_pin.low()

    def switch_backlight(self,state=None):
        if state==True:
            self.backlight_pin.high()
        elif state==False:
            self.backlight_pin.low()
        else:
            if self.backlight_pin.value():
                self.backlight_pin.low()
            else:
                self.backlight_pin.high()

    def get_backlight(self):
        return self.backlight_pin.value()

    def display(self,img,x=0,y=0):
        global display_high_speed,zoom_scale_w,zoom_scale_h
        display_with_battery(img)
        if display_high_speed==0:
            lcd.display(img,x,y,x_size=320)
            zoom_scale_w=img.width()/320
            zoom_scale_h=img.height()/240
        elif display_high_speed==1:
            lcd.display(img,x,y,x_size=240)
            zoom_scale_w=img.width()/240
            zoom_scale_h=img.height()/180
        elif display_high_speed==2:
            lcd.display(img,x,y,x_size=160)
            zoom_scale_w=img.width()/160
            zoom_scale_h=img.height()/120

    def 显示(self,img,x=0,y=0):
        global display_high_speed
        display_with_battery(img)
        if display_high_speed==0:
            lcd.display(img,x,y,x_size=320)
        elif display_high_speed==1:
            lcd.display(img,x,y,x_size=240)
        elif display_high_speed==2:
            lcd.display(img,x,y,x_size=160)
    def touch_default_value(self):
        for n in range(2):
            uart.writechar(0x47)
            uart.writechar(0x65)
            uart.writechar(23)
            for n in range(0,20):
                uart.writechar(touch_calibration_data[n])
                touch_calibration_data[20] = touch_calibration_data[20]^touch_calibration_data[n]
            uart.writechar(touch_calibration_data[20])
            sleep(0.5)

    def touch_calibration(self,img):
        global x_c_lib,y_c_lib,z_c_lib
        global x_min,x_max,x_stepping,y_min,y_max,y_stepping
        global z_min,z_max,z_x_stepping,z_y_stepping,touch_calibration_check
        self.tim3 = Timer(4, freq=1000)
        self.buzzer = self.tim3.channel(4, Timer.PWM, pin=Pin("D15"), pulse_width_percent=0)
        x_c_lib=[40,280,40,280,160]
        y_c_lib=[40,40,20,20,120]
        z_c_lib=[40,40,20,20,120]
        x_c_last=0
        y_c_last=0
        z_c_last=0
        xy_calibration_n=0
        X_lib=(40,280,40,280,160)
        Y_lib=(40,40,200,200,120)
        check_data=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
        for n in range(0,5):
            img.clear()
            img.draw_circle(X_lib[n],Y_lib[n],10)
            img.draw_cross(X_lib[n],Y_lib[n])
            img.draw_rectangle(X_lib[n]-10,Y_lib[n]-22,20,5,color=(100,100,100))
            self.display(img)
            loop=True
            while loop:
                if 200<XX<3600 and 200<YY<3600:
                    if abs(XX-x_c_last)<15 and abs(YY-y_c_last)<15 and abs(ZZ-z_c_last)<25:
                        xy_calibration_n+=1
                        img.draw_rectangle(X_lib[n]-9,Y_lib[n]-21,int(xy_calibration_n/3),3,color=(255,255,0),fill=True)
                    else:
                        img.draw_rectangle(X_lib[n]-9,Y_lib[n]-21,18,3,color=(0,0,0),fill=True)
                        xy_calibration_n=0
                    if xy_calibration_n==43:
                        x_c_lib[n]=XX
                        y_c_lib[n]=YY
                        z_c_lib[n]=ZZ
                    elif 52>xy_calibration_n>43:
                        x_c_lib[n]=(x_c_lib[n]+XX)/2
                        y_c_lib[n]=(y_c_lib[n]+YY)/2
                        z_c_lib[n]=(z_c_lib[n]+ZZ)/2
                    elif xy_calibration_n==53:
                        loop=False
                        print(str(n)+'x:'+str(x_c_lib[n]))
                        print(str(n)+'y:'+str(y_c_lib[n]))
                        print(str(n)+'z:'+str(z_c_lib[n]))
                    x_c_last=XX
                    y_c_last=YY
                    z_c_last=ZZ
                else:
                    img.draw_rectangle(X_lib[n]-9,Y_lib[n]-21,18,3,color=(0,0,0),fill=True)
                    xy_calibration_n=0
                self.display(img)
                sleep(0.005)
            img.draw_rectangle(X_lib[n]-10,Y_lib[n]-22,20,5,color=(0,255,0),fill=True )
            img.draw_circle(X_lib[n],Y_lib[n],10,color=(0,255,0))
            img.draw_cross(X_lib[n],Y_lib[n],color=(0,255,0))
            self.display(img)
            self.buzzer.pulse_width_percent(50)
            self.tim3.freq(1000)
            sleep(0.1)
            self.buzzer.pulse_width_percent(50)
            self.tim3.freq(800)
            sleep(0.1)
            self.buzzer.pulse_width_percent(0)
            self.tim3.freq(800)
            for x in range(0,5):
                while 200<XX<3600 and 200<YY<3600:
                    x=0
                img.clear()
                img.draw_line(X_lib[n]-5,Y_lib[n]-8,X_lib[n]+5,Y_lib[n]+8,color=(255,255,0))
                img.draw_line(X_lib[n]-5,Y_lib[n]-8,X_lib[n]+5,Y_lib[n]-8,color=(255,255,0))
                img.draw_line(X_lib[n]-5,Y_lib[n]+8,X_lib[n]+5,Y_lib[n]-8,color=(255,255,0))
                img.draw_line(X_lib[n]-5,Y_lib[n]+8,X_lib[n]+5,Y_lib[n]+8,color=(255,255,0))
                self.display(img)
                sleep(0.1)
                img.clear()
        #容差超过5像素、中点校验超过15像素则认为失败
        print('获取完成')
        if abs(x_c_lib[0]-x_c_lib[2])>150 or abs(x_c_lib[1]-x_c_lib[3])>150 or \
        abs(y_c_lib[0]-y_c_lib[1])>150 or abs(y_c_lib[2]-y_c_lib[3])>170 or \
        abs((((x_c_lib[0]+x_c_lib[2])/2) - x_c_lib[4]) - (x_c_lib[4] - ((x_c_lib[1]+x_c_lib[3])/2)))>300 or \
        abs((((y_c_lib[2]+y_c_lib[3])/2) - y_c_lib[4]) - (y_c_lib[4] - ((y_c_lib[0]+y_c_lib[1])/2)))>300:
            print('数据校验失败')
            return 'failed'
        else:
            print('数据获取成功')
            x_stepping=( ((x_c_lib[0]+x_c_lib[2])/2) - ((x_c_lib[1]+x_c_lib[3])/2) )/240
            x_min=int(((x_c_lib[1]+x_c_lib[3])/2)-(x_stepping*40))
            x_max=int(((x_c_lib[0]+x_c_lib[2])/2)+(x_stepping*40))
            y_stepping=( ((y_c_lib[2]+y_c_lib[3])/2) - ((y_c_lib[0]+y_c_lib[1])/2) )/160
            y_min=int(((y_c_lib[0]+y_c_lib[1])/2)-(y_stepping*40))
            y_max=int(((y_c_lib[2]+y_c_lib[3])/2)+(y_stepping*40))

            z_x_stepping=(((z_c_lib[1]+z_c_lib[3])/2) - ((z_c_lib[0]+z_c_lib[2])/2))/240
            z_y_stepping=(((z_c_lib[2]+z_c_lib[3])/2) - ((z_c_lib[0]+z_c_lib[1])/2))/160
            z_min=int(z_c_lib[0]-z_x_stepping*40-z_y_stepping*40+150)
            z_max=int(z_c_lib[3]+z_x_stepping*40+z_y_stepping*40+150)
            print('x_min:'+str(x_min)+',x_max:'+str(x_max)+',x_stepping:'+str(x_stepping))
            print('y_min:'+str(y_min)+',y_max:'+str(y_max)+',y_stepping:'+str(y_stepping))
            print('z_min:'+str(z_min)+',z_max:'+str(z_max)+',z_x_stepping:'+str(z_x_stepping)+',z_y_stepping:'+str(z_y_stepping))

            #发送数据 序号23

        check_data[0]=x_min>>8
        check_data[1]=x_min%256
        check_data[2]=x_max>>8
        check_data[3]=x_max%256
        check_data[4]=(int(x_stepping*1000))>>8
        check_data[5]=(int(x_stepping*1000))%256

        check_data[6]=y_min>>8
        check_data[7]=y_min%256
        check_data[8]=y_max>>8
        check_data[9]=y_max%256
        check_data[10]=(int(y_stepping*1000))>>8
        check_data[11]=(int(y_stepping*1000))%256

        check_data[12]=z_min>>8
        check_data[13]=z_min%256

        check_data[14]=z_max>>8
        check_data[15]=z_max%256

        check_data[16]=(int(z_x_stepping*1000))>>8
        check_data[17]=(int(z_x_stepping*1000))%256

        check_data[18]=(int(z_y_stepping*1000))>>8
        check_data[19]=(int(z_y_stepping*1000))%256
        check_data[20]=23

        uart.writechar(0x47)
        uart.writechar(0x65)
        uart.writechar(23)

        for n in range(0,20):
            uart.writechar(check_data[n])
            check_data[20] = check_data[20]^check_data[n]
        uart.writechar(check_data[20])

        sleep(0.05)
        timeout=5
        touch_calibration_check=0
        while timeout>0 and touch_calibration_check==0:
            timeout-=1
            uart_send_function(27)
            sleep(0.01)
        if touch_calibration_check!=check_data[20]:
            return 'communication_error'
            print('通讯校验失败')
        else:
            return 'success'

    def get_touch(self):
        global x,y,x_min,XX,x_max,x_stepping,y_min,YY,y_max,y_stepping
        global ZZ,z_min,z_x_stepping,z_y_stepping,z_max,display_high_speed
        global zoom_scale_w,zoom_scale_h
        screen_get=0
        if x_min<XX<x_max:
            x=(XX-x_min)/x_stepping
            if x>320:x=320
            elif x<0:x=0
            x=320-x
            screen_get+=1
        if y_min<YY<y_max:
            y=(YY-y_min)/y_stepping
            if y>240:y=240
            elif y<0:y=0
            screen_get+=1
        if screen_get<2 or ZZ> (((z_min+(z_x_stepping*x)+(z_y_stepping*y)) + (z_max-(z_x_stepping*(320-x))-(z_y_stepping*(240-y))) )/2):
            x=y=-1
        else:
            if display_high_speed==0:
                x=x*zoom_scale_w
                y=y*zoom_scale_h
            elif display_high_speed==1:
                if 40<x<280:
                    x=(x-40)*zoom_scale_w
                    print(x)
                else:
                    x=-1
                if 30<y<210:
                    y=(y-30)*zoom_scale_h
                else:
                    y=-1
            elif display_high_speed==2:
                if 80<x<240:
                    x=(x-80)*zoom_scale_w
                else:
                    x=-1
                if 60<y<180:
                    y=(y-60)*zoom_scale_h
                else:
                    y=-1
        self.x=round(x)
        self.y=round(y)
        return (self.x,self.y)

    def touch_x(self):
        return self.x

    def touch_y(self):
        return self.y

    def touch_z(self):
        global ZZ,z_min,z_x_stepping,z_y_stepping
        global x,y
        return 4096-(ZZ-z_x_stepping*x-z_y_stepping*y)

    def touch_exist(self):
        if self.x>=0 and self.y>=0:
            return True
        else:
            return False
#按键：输入尺寸，类型分为抬起有效和按下有效，设置是否显示边框，边框颜色。检测按键状态时装入image即可在画面显示边框。抬起有效，按下时，后续显示不能刷新。
class touch_button(screen):
    def __init__(self,b_x,b_y,b_w,b_h,type='release_valid',tone=True,draw_frame=True,frame_color=(255,255,255)):#触摸按键
        self.b_x=b_x
        self.b_y=b_y
        self.b_w=b_w
        self.b_h=b_h
        self.draw_frame=draw_frame
        self.frame_color=frame_color
        self.tone=tone
        if tone:
            self.tim3 = Timer(4, freq=1000)
            self.buzzer = self.tim3.channel(4, Timer.PWM, pin=Pin("D15"), pulse_width_percent=0)
        self.button_press=False
        if type=='release_valid':
            self.type=1
        elif type=='press_valid':
            self.type=2

    def state(self,img=alloc_extra_fb(1,1,3)):
        global x,y,select_rect_busy,display_high_speed
        if not select_rect_busy:
            if display_high_speed!=0:
                thickness=2
            else:
                thickness=1
            sound_time=0
            loop=0# >0为有效循环
            if self.draw_frame and img.width()>10:#画按键框
                img.draw_rectangle(self.b_x,self.b_y,self.b_w,self.b_h,thickness=thickness,color=self.frame_color)
            self.get_touch()#获取触摸坐标
            if self.b_x<x<(self.b_x+self.b_w) and self.b_y<y<(self.b_y+self.b_h):
                if self.tone and not self.button_press:
                    self.buzzer.pulse_width_percent(50)
                    self.tim3.freq(500)
                    sleep(0.007)
                    self.buzzer.pulse_width_percent(0)
                    self.tim3.freq(500)
                if self.draw_frame and img.width()>10:#画按键框
                    img.draw_rectangle(self.b_x,self.b_y,self.b_w,self.b_h,thickness=thickness,color=(255,0,0))
                    if self.type==1:#抬起有效时才刷新显示
                        self.display(img)
                loop=10
                self.button_press=True
    #抬起有效
                if self.type==1:
                    sleep(0.02)#20ms按下消抖
                    if x>0 and y>0:#20ms后还存在按下，则等待抬起
                        while loop>0:
                            sleep(0.002)
                            self.get_touch()
                            if x>0 and y>0:
                                loop=10
                            loop-=1
                    else:#20ms后没有按下，则视为没有按下
                        self.button_press=False
                    if self.button_press:#有按下的前提，执行抬起动作
                        self.button_press=False
                        if self.tone:
                            self.buzzer.pulse_width_percent(50)
                            self.tim3.freq(600)
                            sleep(0.007)
                            self.buzzer.pulse_width_percent(0)
                            self.tim3.freq(600)
                        if self.draw_frame and img.width()>10:
                            img.draw_rectangle(self.b_x,self.b_y,self.b_w,self.b_h,thickness=thickness,color=self.frame_color)
                            self.display(img)
                        return True
                    else:
                        return False
        else:
            return False
#按下有效
        if self.type==2:
            if loop:
                return True
            else:
                if self.button_press:#首次抬起
                    self.button_press=False
                    if self.tone:
                        self.buzzer.pulse_width_percent(50)
                        self.tim3.freq(600)
                        sleep(0.007)
                        self.buzzer.pulse_width_percent(0)
                        self.tim3.freq(600)
                return False
        else:
            pass

class touch_slider(screen):
    def __init__(self,img,sx,sy,sw,sh,color=(255,255,255),default_value=0,background_color=(0,0,0)):
        self.sx=sx
        self.sy=sy
        self.sw=sw
        self.sh=sh
        c=0
        if default_value<0:
            default_value=0
        elif default_value>=100:
            default_value=100
            c=-2
        self.touch_slider_n=default_value
        self.slider_w=int((self.sw-4)/10)
        self.color=color
        self.background_color=background_color
        img.draw_rectangle(self.sx,self.sy,self.sw,self.sh,color=color)
        img.draw_rectangle(self.sx+1,self.sy+1,self.sw-2,self.sh-2,color=self.background_color,fill=True)#清除
        img.draw_rectangle(int(self.sx+2+c+(default_value/100)*int(sw*0.9)),self.sy+2,self.slider_w,self.sh-4,color=self.color,fill=True) #画滑块

    def value(self,img,refresh=False,value=None):
        global x,y
        self.get_touch()
        if refresh:
            x=self.sx+1
            y=self.sy+1
        if value!=None:
            if value<1:
                x=self.sx+1
            elif value>99:
                x=self.sx+self.sw-1
            else:
                x=self.sx+round(self.sw*value/100)
            y=self.sy+1
        if self.sx<x<(self.sx+self.sw) and self.sy<y<(self.sy+self.sh):
            img.draw_rectangle(self.sx,self.sy,self.sw,self.sh,color=self.color) #画外框
            img.draw_rectangle(self.sx+1,self.sy+1,self.sw-2,self.sh-2,color=self.background_color,fill=True)#清除
            self.touch_slider_n=(x-(self.sx+self.sw*0.05))/(self.sw*0.9)*100
            if self.touch_slider_n<0:
                self.touch_slider_n=0
                block_x=self.sx+2
            elif self.touch_slider_n>100:
                self.touch_slider_n=100
                block_x=self.sx+self.sw-self.slider_w-2
            else:
                block_x=self.sx+1+int(self.touch_slider_n/100*self.sw*0.9)
            img.draw_rectangle(block_x,self.sy+2,self.slider_w,self.sh-4,color=self.color,fill=True) #画滑块
            return self.touch_slider_n
        else:
            return self.touch_slider_n

class touch_select_rect(screen):
    def __init__(self,x=140,y=100,w=40,h=40,color=(255,255,255),w_min=20,h_min=20):
        self.xx=x
        self.yy=y
        if w<w_min:
            self.ww=w_min
        else:
            self.ww=w
        if h<h_min:
            self.hh=h_min
        else:
            self.hh=h
        self.color=color
        self.touch_first_time=True
        self.function=0 #功能1：改变框尺寸，功能2：整体移动
        self.first_time_x=-1#首次按下的坐标
        self.first_time_y=-1
        self.first_time_box_x=self.xx
        self.first_time_box_y=self.yy
        self.first_time_box_w=self.ww
        self.first_time_box_h=self.hh
        self.w_min=w_min
        self.h_min=h_min
    def value(self,img):
        global select_rect_busy,zoom_scale_w,zoom_scale_h
        self.get_touch()
        if self.touch_exist():
            if self.touch_first_time:#首次开始触摸
                if self.xx+self.ww-10<self.x<self.xx+self.ww+10 and self.yy+self.hh-10<self.y<self.yy+self.hh+10:
                    self.function=1#改变尺寸模式
                    self.first_time_x=self.x
                    self.first_time_y=self.y
                    self.touch_first_time=False
                elif self.xx<self.x<self.xx+self.ww and self.yy<self.y<self.yy+self.hh:
                    self.function=2#移动整体模式
                    self.first_time_x=self.x
                    self.first_time_y=self.y
                    self.touch_first_time=False
            else:#非首次触摸(已有触摸)
                select_rect_busy=True
                if self.function==1:#改变尺寸
                    self.ww = self.first_time_box_w+self.x-self.first_time_x
                    self.hh = self.first_time_box_h+self.y-self.first_time_y
                    if self.ww<self.w_min:
                        self.ww=self.w_min
                    if self.hh<self.h_min:
                        self.hh=self.h_min
                elif self.function==2:#整体移动
                    self.xx = self.first_time_box_x+self.x-self.first_time_x
                    self.yy = self.first_time_box_y+self.y-self.first_time_y
                    if self.xx<0:
                        self.xx=0
                    elif self.xx>int(320*zoom_scale_w)-1-self.ww:
                        self.xx=int(320*zoom_scale_w)-1-self.ww
                    if self.yy<0:
                        self.yy=0
                    elif self.yy>int(240*zoom_scale_h)-1-self.hh:
                        self.yy=int(240*zoom_scale_h)-1-self.hh
        else:#没有触摸，首次触摸状态复位
            select_rect_busy=False
            self.touch_first_time=True
            self.first_time_box_x=self.xx
            self.first_time_box_y=self.yy
            self.first_time_box_w=self.ww
            self.first_time_box_h=self.hh
        img.draw_line(self.xx,self.yy,self.xx+self.ww,self.yy,color=self.color)
        img.draw_line(self.xx,self.yy,self.xx,self.yy+self.hh,color=self.color)
        img.draw_line(self.xx+self.ww,self.yy,self.xx+self.ww,self.yy+self.hh-10,color=self.color)
        img.draw_line(self.xx,self.yy+self.hh,self.xx+self.ww-10,self.yy+self.hh,color=self.color)
        img.draw_rectangle(self.xx+self.ww-10,self.yy+self.hh-10,20,20,color=self.color)
        return (self.xx,self.yy,self.ww,self.hh)
    def busy(self):
        global select_rect_busy
        return select_rect_busy

class keyboard(screen):
    def __init__(self,tone=True,realtime=False):
        self.keyboard_keys=(('q','w','e','r','t','y','u','i','o','p','a','s','d','f','g','h','j','k','l','z','x','c','v','b','n','m'),\
('Q','W','E','R','T','Y','U','I','O','P','A','S','D','F','G','H','J','K','L','Z','X','C','V','B','N','M'),\
('1','2','3','4','5','6','7','8','9','0','+','-','*','/','%','&',"|",'!','^','<','>','(',')','~','=','.'),\
('$','@','&','/',chr(0x5c),'`',',','.','?','!','#','[',']','{','}',"'",'"',':',';','<','>','(',')','~','-','_'))
        self.function_keys=['ABC','Del','123','@&#','Enter']
        if tone:
            self.tim3 = Timer(4, freq=1000)
            self.buzzer = self.tim3.channel(4, Timer.PWM, pin=Pin("D15"), pulse_width_percent=0)
        self.keyboard_ton=tone
        self.realtime=realtime
        self.refresh_display=True
        self.mode=0
        #Function Keys Coordinates
        self.FKC=((7,41,180,204),(278,312,180,204),(7,41,210,234),(45,79,210,234),(239,313,210,234),(84,235,210,234))

    def draw_keyboard(self,img):
        self.x_start=(7,22,53,0)
        key_num=0
        for y_n in range(120,210,30):#26键
            x_start_n=self.x_start[int((y_n-120)/30)]
            for x_n in range( x_start_n , 319-x_start_n , 31 ):
                img.draw_rectangle(x_n,y_n,27,24,color=(106,106,106),fill=True)
                text.word(img,x_n+8,y_n+2,self.keyboard_keys[self.mode][key_num])
                key_num+=1
        if self.mode>0:
            self.function_keys[0]='abc'
        else:
            self.function_keys[0]='ABC'
        for n in range(0,5):
            img.draw_rectangle(self.FKC[n][0],self.FKC[n][2],self.FKC[n][1]-self.FKC[n][0],self.FKC[n][3]-self.FKC[n][2],color=(71,71,71),fill=True)#大写 小写 字母
            text.word(img,self.FKC[n][0]+3+int(n/4)*12,self.FKC[n][2]+2,self.function_keys[n])
        img.draw_rectangle(84,210,151,24,color=(106,106,106),fill=True)#空格
        self.display(img)

    def key_tone(self,mode):
        if self.keyboard_ton:
            if mode:
                self.buzzer.pulse_width_percent(50)
                self.tim3.freq(500)
                sleep(0.007)
                self.buzzer.pulse_width_percent(0)
                self.tim3.freq(500)
            else:
                self.buzzer.pulse_width_percent(50)
                self.tim3.freq(600)
                sleep(0.007)
                self.buzzer.pulse_width_percent(0)
                self.tim3.freq(600)
    def waiting_release(self):
        while self.touch_exist():
            self.get_touch()
    def input(self,img,refresh=False):
        if self.refresh_display:
            if self.realtime:
                img.draw_rectangle(0,113,320,156,color=(44,44,44),fill=True)
            else:
                img.draw_rectangle(0,84,320,156,color=(44,44,44),fill=True)
            self.draw_keyboard(img)
            input_text=''
        if self.realtime:
            self.refresh_display=refresh
        loop=True
        cursor_color=255
        cursor_time=millis()
        while loop:
            loop=not self.realtime
            #光标
            if millis()-cursor_time>500 and not self.realtime:
                cursor_time=millis()
                if cursor_color==255:
                    cursor_color=44
                else:
                    cursor_color=255
                img.draw_line(9+len(input_text)*9,93,9+len(input_text)*9,110,color=(cursor_color,cursor_color,cursor_color))
                self.display(img)
            #获取键值
            first_press=0
            self.get_touch()
            if self.touch_exist():
                key_num=0
                press_key_num=-1    #键值
                for y_n in range(120,210,30):#26键
                    x_start_n=self.x_start[int((y_n-120)/30)]
                    for x_n in range( x_start_n,319-x_start_n,31):
                        if x_n<self.x<(x_n+27) and y_n<self.y<(y_n+24):
                            press_key_num=key_num
                            break
                        key_num+=1
                    if press_key_num==key_num:
                        break
                for n in range(0,6):#功能键
                    if self.FKC[n][0]<self.x<self.FKC[n][1] and self.FKC[n][2]<self.y<self.FKC[n][3]:
                        press_key_num=key_num
                        #print(press_key_num)
                        break
                    key_num+=1
                #根据键值操作
                if press_key_num>-1:#键值-操作
                    if first_press==0:
                        self.key_tone(True)
                        if press_key_num<26:#字母
                            if self.realtime:
                                self.waiting_release()
                                return self.keyboard_keys[self.mode][press_key_num]
                            else:
                                text.word(img,x_n+8,y_n+2,self.keyboard_keys[self.mode][press_key_num],color=(71,71,71))
                                img.draw_line(9+len(input_text)*9,93,9+len(input_text)*9,110,color=(44,44,44))
                                input_text+=self.keyboard_keys[self.mode][press_key_num]
                                text.word(img,8,92,input_text)
                                self.display(img)
                        elif press_key_num==26:#大小写
                            if self.mode==0:
                                self.mode=1
                            else:
                                self.mode=0
                            self.draw_keyboard(img)
                        elif press_key_num==27:#删除
                            if self.realtime:
                                self.waiting_release()
                                return 'del'
                            else:
                                input_text=input_text[:-1]
                                img.draw_rectangle(8+len(input_text)*9,92,11,20,color=(44,44,44),fill=True)
                                self.display(img)
                        elif press_key_num==28:#数字
                            self.mode=2
                            self.draw_keyboard(img)
                        elif press_key_num==29:#标点
                            self.mode=3
                            self.draw_keyboard(img)
                        elif press_key_num==30:#Enter
                            if self.realtime:
                                self.waiting_release()
                                return 'enter'
                            else:
                                return input_text
                        elif press_key_num==31:#空格
                            if self.realtime:
                                self.waiting_release()
                                return ' '
                            else:
                                img.draw_line(9+len(input_text)*9,93,9+len(input_text)*9,110,color=(44,44,44))
                                input_text+=' '
                                text.word(img,8,92,input_text)
                                self.display(img)
                        first_press=1
                    self.waiting_release()
                if first_press==1:
                    self.key_tone(False)
                    if press_key_num<26 and not self.realtime:#字母
                        text.word(img,x_n+8,y_n+2,self.keyboard_keys[self.mode][press_key_num])
                        self.display(img)
                    first_press=2

class drive:
    def __init__(self):
        self.tim = Timer(2, freq=10000)
        self.motor1_ccw = self.tim.channel(1, Timer.PWM, pin=Pin("A0"), pulse_width_percent=0)
        self.motor1_cw = self.tim.channel(2, Timer.PWM, pin=Pin("A1"), pulse_width_percent=0)
        self.motor2_cw = self.tim.channel(3, Timer.PWM, pin=Pin("A2"), pulse_width_percent=0)
        self.motor2_ccw = self.tim.channel(4, Timer.PWM, pin=Pin("A3"), pulse_width_percent=0)
        self.tim2 = Timer(3, freq=10000)
        self.motor3_ccw = self.tim2.channel(1, Timer.PWM, pin=Pin("B4"), pulse_width_percent=0)
        self.motor3_cw = self.tim2.channel(2, Timer.PWM, pin=Pin("B5"), pulse_width_percent=0)
        self.motor4_cw = self.tim2.channel(3, Timer.PWM, pin=Pin("B0"), pulse_width_percent=0)
        self.motor4_ccw = self.tim2.channel(4, Timer.PWM, pin=Pin("B1"), pulse_width_percent=0)

    def motor(self,n,power,brake=100):
        if power>100:power=100
        elif power<-100:power=-100
        power = int(power)
        if n==0:
            if power>0:
                self.motor1_cw.pulse_width_percent(100)
                self.motor1_ccw.pulse_width_percent(100-power)
            elif power==0:
                self.motor1_cw.pulse_width_percent(brake)
                self.motor1_ccw.pulse_width_percent(brake)
            else:
                self.motor1_cw.pulse_width_percent(100-int(power*-1))
                self.motor1_ccw.pulse_width_percent(100)
        elif n==1:
            power*=-1
            if power>0:
                self.motor2_cw.pulse_width_percent(100)
                self.motor2_ccw.pulse_width_percent(100-power)
            elif power==0:
                self.motor2_cw.pulse_width_percent(brake)
                self.motor2_ccw.pulse_width_percent(brake)
            else:
                self.motor2_cw.pulse_width_percent(100-int(power*-1))
                self.motor2_ccw.pulse_width_percent(100)
        elif n==2:
            power*=-1
            if power>0:
                self.motor3_cw.pulse_width_percent(100)
                self.motor3_ccw.pulse_width_percent(100-power)
            elif power==0:
                self.motor3_cw.pulse_width_percent(brake)
                self.motor3_ccw.pulse_width_percent(brake)
            else:
                self.motor3_cw.pulse_width_percent(100-int(power*-1))
                self.motor3_ccw.pulse_width_percent(100)
        elif n==3:
            if power>0:
                self.motor4_cw.pulse_width_percent(100)
                self.motor4_ccw.pulse_width_percent(100-power)
            elif power==0:
                self.motor4_cw.pulse_width_percent(brake)
                self.motor4_ccw.pulse_width_percent(brake)
            else:
                self.motor4_cw.pulse_width_percent(100-int(power*-1))
                self.motor4_ccw.pulse_width_percent(100)

    def move(self,LR,FB,turn):
        move_sum = abs(FB)+abs(LR)+abs(turn)
        if move_sum>=100:
            k=100/move_sum
            FB=FB*k
            LR=LR*k
            turn=turn*k
        self.motor(0,FB+LR*1+turn)     #左前
        self.motor(1,FB+LR*-1+turn)     #左后
        self.motor(2,FB+LR*-1+turn*-1)    #右前
        self.motor(3,FB+LR*1+turn*-1)   #右后

#电机编码器中断程序#
m0a = Pin('PE8',Pin.IN)
m0b = Pin('PE9',Pin.IN)
m1a = Pin('PE12',Pin.IN)
m1b = Pin('PE13',Pin.IN)
m2a = Pin('PE10',Pin.IN)
m2b = Pin('PE11',Pin.IN)
m3a = Pin('PE14',Pin.IN)
m3b = Pin('PE15',Pin.IN)
encoder=[0,0,0,0]

def m0_encoder(nn):
    global encoder
    if m0a.value()==m0b.value():
        encoder[0]+=1
    else:
        encoder[0]-=1

def m1_encoder(nn):
    global encoder
    if m1a.value()==m1b.value():
        encoder[1]-=1
    else:
        encoder[1]+=1

def m2_encoder(nn):
    global encoder
    if m2a.value()==m2b.value():
        encoder[2]+=1
    else:
        encoder[2]-=1

def m3_encoder(nn):
    global encoder
    if m3a.value()==m3b.value():
        encoder[3]+=1
    else:
        encoder[3]-=1
#电机闭环驱动
class drive_close_loop(drive):
    def __init__(self):#初始化编码器的外中断及定时器中断，初始化电机驱动
        self.m1_extint = ExtInt('PE9',ExtInt.IRQ_RISING_FALLING,Pin.PULL_UP,m0_encoder)
        self.m2_extint = ExtInt('PE12',ExtInt.IRQ_RISING_FALLING,Pin.PULL_UP,m1_encoder)
        self.m3_extint = ExtInt('PE10',ExtInt.IRQ_RISING_FALLING,Pin.PULL_UP,m2_encoder)
        self.m4_extint = ExtInt('PE14',ExtInt.IRQ_RISING_FALLING,Pin.PULL_UP,m3_encoder)
        drive.__init__(self)
        motor_interrupt=Timer(7,freq=200)
        motor_interrupt.callback(self.motor_close_loop)
        self.motor_lock=[True,True,True,True]#是否锁定电机角度
        self.get_speed_frequency=2
        self.get_speed_times=99
        self.actual_speed_count=0#临时变量用于计次
        self.actual_speed=[0,0,0,0]#电机实际速度
        self.motor_offset=[0,0,0,0]#电机角度补偿量
        self.close_loop_speed=[0,0,0,0]#速度闭环值
        self.degrees_running=[False,False,False,False]#是否在运行电机角度闭环
        self.degrees_target=[0,0,0,0]#角度环目标值
        self.degrees_speed=[0,0,0,0]#角度环的速度
        self.encoder_last=[0,0,0,0]#用于测速
        self.motor_speed_lock=[0,0,0,0]#用于速度模式的角度自锁判断
#以200HZ频率运行的电机闭环程序
    def motor_close_loop(self,nn):
        global encoder
        #速度环
        self.motor_offset[0]+=self.close_loop_speed[0]
        self.motor_offset[1]+=self.close_loop_speed[1]
        self.motor_offset[2]+=self.close_loop_speed[2]
        self.motor_offset[3]+=self.close_loop_speed[3]
        #角度环
        for n in range(0,4):
            if self.degrees_running[n]:
                if self.degrees_speed[n]>0:#正转
                    if self.degrees_target[n]>encoder[n]:
                        self.motor_offset[n]+=self.degrees_speed[n]
                    else:
                        self.motor_offset[n]=self.degrees_target[n]
                        self.degrees_running[n]=False
                else:#反转
                    if self.degrees_target[n]<encoder[n]:
                        self.motor_offset[n]+=self.degrees_speed[n]
                    else:
                        self.motor_offset[n]=self.degrees_target[n]
                        self.degrees_running[n]=False
            if  self.motor_lock[n] or self.degrees_running[n] or self.close_loop_speed[n]!=0:
                self.motor(n,(encoder[n]-self.motor_offset[n])*-10)
            elif not self.motor_lock[n]:
                self.motor_offset[n]=encoder[n]
            else:
                self.motor(n,0,brake=0)
                self.motor_offset[n]=encoder[n]

        #self.motor(1,(encoder[1]-self.motor_offset[1])*-10)
        #self.motor(2,(encoder[2]-self.motor_offset[2])*-10)
        #self.motor(3,(encoder[3]-self.motor_offset[3])*-10)
        #获取实际速度
        self.actual_speed_count+=1
        if self.actual_speed_count>self.get_speed_times:
            self.actual_speed_count=0
            self.actual_speed[0]=(encoder[0]-self.encoder_last[0])*self.get_speed_frequency
            self.actual_speed[1]=(encoder[1]-self.encoder_last[1])*self.get_speed_frequency
            self.actual_speed[2]=(encoder[2]-self.encoder_last[2])*self.get_speed_frequency
            self.actual_speed[3]=(encoder[3]-self.encoder_last[3])*self.get_speed_frequency
            self.encoder_last[0]=encoder[0]
            self.encoder_last[1]=encoder[1]
            self.encoder_last[2]=encoder[2]
            self.encoder_last[3]=encoder[3]
#获取电机速度，单位 厘米/秒
    def get_motor_speed(self,n):
        return self.actual_speed[n]*23.876/168
#获取电机角速度，单位 度/秒
    def get_motor_degrees_speed(self,n):
        return self.actual_speed[n]*360/168
#获取电机总角度，单位 度
    def get_motor_degrees(self,n):
        global encoder
        return encoder[n]*360/168
#获取电机总里程，单位 厘米
    def get_motor_mileage(self,n):
        global encoder
        return encoder[n]*23.876/168
#单电机控制转到角度，n=端口号（0~3），speed=速度（单位度/秒），degrees=角度（单位度），waiting_done=是否等待完成运转
#注：不等待完成运转的话，程序可以执行其他语句。但下次执行同一电机会等待上一次指令完成。
    def motor_degrees(self,n,speed,degrees,waiting_done=True):
        if speed*degrees==0:
            return False
        elif speed*degrees<0: #如果有一方为负，则反转，将另一数值也调为负数(运行时需要目标和速度符号相同)
            if speed>0:speed*=-1
            elif degrees>0:degrees*=-1
        elif speed<0 and degrees<0: #如果双方都为负，则正转，将双方都调为正数
            speed*=-1
            degrees*=-1
        speed = speed*168/72000
        degrees = degrees*168/360

        self.close_loop_speed[n]=0#恒速模式值置零
        while self.degrees_running[n]:
            pass
        self.degrees_speed[n]=speed
        self.degrees_target[n]=self.motor_offset[n]+degrees
        self.degrees_running[n]=True
        if waiting_done:
            while self.degrees_running[n]:
                pass
#单电机控制速度，n=端口号（0~3），speed=速度（单位度/秒）
    def motor_speed(self,n,speed):
        global encoder

        self.close_loop_speed[n]=speed*168/72000
        if speed==0:#首次进入，才将角度输入补偿角度，以锁定电机
            if self.motor_speed_lock==0:
                self.motor_speed_lock=1
                self.motor_offset[n]=encoder[n]
        else: self.motor_speed_lock=0
#是否锁定电机角度，n=端口号（0~3），nn=是否
    def motor_lock_degrees(self,n,nn):
        self.motor_lock[n]=nn
#是否锁定电机角度，n=端口号（0~3），nn=是否
    def lock_degrees(self,nn):
        self.motor_lock[0]=nn
        self.motor_lock[1]=nn
        self.motor_lock[2]=nn
        self.motor_lock[3]=nn

#停车时是否制动
    def brake_when_stop(self,n):
        self.motor_lock[0]=n
        self.motor_lock[1]=n
        self.motor_lock[2]=n
        self.motor_lock[3]=n

#设置获取速度的频率，频率越高精度越低
    def set_speed_refresh(self,nn):
        self.get_speed_frequency=nn
        self.get_speed_times=200/nn-1
#停止所有电机
    def stop(self):
        global encoder
        self.close_loop_speed=[0,0,0,0]
        self.motor_offset[0]=encoder[0]
        self.motor_offset[1]=encoder[1]
        self.motor_offset[2]=encoder[2]
        self.motor_offset[3]=encoder[3]
        self.degrees_running=[False,False,False,False]
#普通驱动模式：x=平移速度（单位cm/s），y=前后速度（单位cm/s）,r自转速度（单位度/秒）
    def move_close_loop(self,x,y,r):
        x=x*360/23.876
        y=y*360/23.876
        r=r*1160.53/360
        self.motor_speed(0,y+x+r)
        self.motor_speed(1,y-x+r)
        self.motor_speed(2,y-x-r)
        self.motor_speed(3,y+x-r)
#角度驱动模式：speed=行进速度（单位cm/s）,degrees=角度，rotation=自转速度（单位度/秒）
    def move_by_direction(self,speed,degrees,rotation):
        speed=speed*360/23.876
        rotation=rotation*1160.53/360
        if degrees==0:
            y=speed
            x=0
        if degrees>360:
            degrees=degrees%360
        if 0<degrees<=90:
            degrees=radians(degrees)
            x=sin(degrees)*speed
            y=cos(degrees)*speed
        elif 90<degrees<=180:
            degrees=radians(degrees-90)
            y=sin(degrees)*speed*-1
            x=cos(degrees)*speed
        elif 180<degrees<=270:
            degrees=radians(degrees-180)
            x=sin(degrees)*speed*-1
            y=cos(degrees)*speed*-1
        elif 270<degrees<=360:
            degrees=radians(degrees-270)
            y=sin(degrees)*speed
            x=cos(degrees)*speed*-1
        self.motor_speed(0,y+x+rotation)
        self.motor_speed(1,y-x+rotation)
        self.motor_speed(2,y-x-rotation)
        self.motor_speed(3,y+x-rotation)
#移动至目的地：x=相对x坐标（单位cm），y=相对y坐标（单位cm），speed=速度（单位cm/s）
    def move_to_destination(self,x,y,speed,waiting_done=False):
        if x!=0 and y!=0:
            speed=(abs(y+x)+abs(y-x))/((x*x+y*y)**0.5/speed)#计算实际斜线速度，
        x=x*360/23.876
        y=y*360/23.876
        speed=speed*360/23.876
        if y!=0 and x!=y:
            speed1=abs(y-x)/(abs(y+x)+abs(y-x))*speed
            speed2=abs(y+x)/(abs(y+x)+abs(y-x))*speed
        else:
            speed1=speed2=speed
        self.motor_degrees(0,speed2,y+x,waiting_done=False)
        self.motor_degrees(1,speed1,y-x,waiting_done=False)
        self.motor_degrees(2,speed1,y-x,waiting_done=False)
        self.motor_degrees(3,speed2,y+x,waiting_done=False)
        if waiting_done:
            while self.degrees_running[0] or self.degrees_running[1] or self.degrees_running[2] or self.degrees_running[3]:
                pass
#旋转值角度：rotation=顺时针旋转的相对角度（单位度），speed=旋转速度（单位度/秒）
    def turn_to_degrees(self,rotation,speed,waiting_done=False):
        rotation=rotation*1160.53/360
        speed=speed*1160.53/360
        self.motor_degrees(0,speed,rotation,waiting_done=False)
        self.motor_degrees(1,speed,rotation,waiting_done=False)
        self.motor_degrees(2,speed,-rotation,waiting_done=False)
        self.motor_degrees(3,speed,-rotation,waiting_done=False)
        if waiting_done:
            while self.degrees_running[0] or self.degrees_running[1] or self.degrees_running[2] or self.degrees_running[3]:
                pass
class led():
    def __init__(self,brightness=90,led_freq=1000):
        self.tim4 = Timer(14, freq=led_freq)
        self.light = self.tim4.channel(1, Timer.PWM, pin=Pin("A7"), pulse_width_percent=0)
        self.brightness=brightness

    def set_brightness(self,n):
        if n<0:n=0
        elif n>100:n=100
        self.light.pulse_width_percent(n)
        if n>0:
            self.brightness=n

    def get_brightness(self):
        return self.light.pulse_width_percent()

    def turn_on(self):
        self.light.pulse_width_percent(self.brightness)

    def turn_off(self):
        self.light.pulse_width_percent(0)

    def switch(self,state=None):
        if state==True:
            self.light.pulse_width_percent(self.brightness)
        elif state==False:
            self.light.pulse_width_percent(0)
        else:
            if self.light.pulse_width_percent()==0:
                self.light.pulse_width_percent(self.brightness)
            else:
                self.light.pulse_width_percent(0)

class buzzer():
    def __init__(self):
        self.tim3 = Timer(4, freq=500)
        self.buzzer = self.tim3.channel(4, Timer.PWM, pin=Pin("D15"), pulse_width_percent=0)

    def frequency(self,hz,volume=60):
        if hz<1 or volume==0:
            hz=1
            self.buzzer.pulse_width_percent(0)
        else:
            hz=int(hz)
            volume=int(volume)
            self.buzzer.pulse_width_percent(0)
            self.tim3.freq(hz)
            self.buzzer.pulse_width_percent(volume)
            self.tim3.freq(hz)

class gamepad():
    def __init__(self):
        self.ON=1
        self.OFF=0
        self.power(self.ON)

    def power(self,switch):#开关手柄返回数据功能
        global gamepad_online,gamepad_receive
        if switch:#开启
            gamepad_receive=0
            timeout=20
            while timeout>0 and gamepad_receive==0:
                timeout-=1
                uart_send_function(1)
                sleep(0.04)
                gamepad_online=0
            if timeout==0:
                raise Exception('错误!未连接手柄!')
        else:#关闭
            timeout=5
            while timeout>0 and gamepad_receive==1:
                timeout-=1
                uart_send_function(2)
                gamepad_receive=0
                sleep(0.04)
                gamepad_online=0
            if timeout==0:
                raise Exception('错误E2')
    def L_X(self):
        if gamepad_data[4]>129:
            return int((gamepad_data[4]-129)/1.26*gamepad_online)
        else:
            return int((gamepad_data[4]-129)/1.28*gamepad_online)
    def R_Y(self):
        if gamepad_data[3]<127:
            return int((127-gamepad_data[3])/1.26*gamepad_online)
        else:
            return int((127-gamepad_data[3])/1.28*gamepad_online)
    def L_Y(self):
        if gamepad_data[5]<127:
            return int((127-gamepad_data[5])/1.26*gamepad_online)
        else:
            return int((127-gamepad_data[5])/1.28*gamepad_online)
    def R_X(self):
        if gamepad_data[2]>129:
            return int((gamepad_data[2]-129)/1.26*gamepad_online)
        else:
            return int((gamepad_data[2]-129)/1.28*gamepad_online)
    def SELECT(self):
        return bool(not(gamepad_data[0]&0x01) and gamepad_online)
    def START(self):
        return bool(not(gamepad_data[0]&0x08) and gamepad_online)
    def UP(self):
        return bool(not(gamepad_data[0]&0x10) and gamepad_online)
    def DOWN(self):
        return bool(not(gamepad_data[0]&0x40) and gamepad_online)
    def LEFT(self):
        return bool(not(gamepad_data[0]&0x80) and gamepad_online)
    def RIGHT(self):
        return bool(not(gamepad_data[0]&0x20) and gamepad_online)
    def L1(self):
        return bool(not(gamepad_data[1]&0x04) and gamepad_online)
    def L2(self):
        return bool(not(gamepad_data[1]&0x01) and gamepad_online)
    def R1(self):
        return bool(not(gamepad_data[1]&0x08) and gamepad_online)
    def R2(self):
        return bool(not(gamepad_data[1]&0x02) and gamepad_online)
    def SQUARE(self):
        return bool(not(gamepad_data[1]&0x80) and gamepad_online)
    def TRIANGLE(self):
        return bool(not(gamepad_data[1]&0x10) and gamepad_online)
    def CIRCLE(self):
        return bool(not(gamepad_data[1]&0x20) and gamepad_online)
    def CROSS(self):
        return bool(not(gamepad_data[1]&0x40) and gamepad_online)
    def vibration(self,n):#使手柄震动，单位秒
        if n>0 and n<0.02:n=0.02
        n=int(n*50)
        uart_send_char(6,n)

class hardware():
    def __init__(self):
        self.ON=True
        self.OFF=False

    def shutdown(self):
        uart_send_function(22)
        uart_send_function(22)
        uart_send_function(22)

    def charge(self,n):
        if n:#开启充电
            timeout=5
            while timeout>0 and charging_state!=1:
                timeout-=1
                uart_send_function(3)
                sleep(0.01)
            if charging_state!=1:
                raise Exception('错误E3')
        else:#关闭充电
            timeout=5
            while timeout>0 and charging_state!=0:
                timeout-=1
                uart_send_function(4)
                sleep(0.01)
            if charging_state!=0:
                raise Exception('错误E4')
    def charge_state(self):
        global charging_state
        timeout=5
        charging_state=2
        while timeout>0 and charging_state==2:
            timeout-=1
            uart_send_function(5)
            sleep(0.01)
        if timeout==0:
            raise Exception('错误E5')
        else:
            return bool(charging_state)

    def set_sales_date(self,password,yy,mm,dd):
        global sales_date
        if password==6150407:
            timeout=5
            sales_date=[0,0,0]
            while timeout>0 and sales_date==[0,0,0]:
                timeout-=1
                uart.writechar(0x47)
                uart.writechar(0x65)
                uart.writechar(15)
                uart.writechar(yy)
                uart.writechar(mm)
                uart.writechar(dd)
                uart.writechar(15^yy^mm^dd)
                sleep(0.05)
            if timeout==0:
                raise Exception('错误E13')
        else:
            print('密码错误！非管理员请勿修改销售日期！')

    def sales_date(self):
        global sales_date
        timeout=5
        sales_date=[0,0,0]
        while timeout>0 and sales_date==[0,0,0]:
            timeout-=1
            uart_send_function(16)
            sleep(0.01)
        if sales_date==[0,0,0]:
            raise Exception('错误E14')
        else:
            return sales_date

    def set_battery_threshold(self,n):
        global battery_threshold
        timeout=5
        while battery_reference==0 and timeout>0:
            timeout-=1
            sleep(1.1)
        if battery_reference!=0:
            timeout=5
            battery_threshold=0
            if n>7.2:n=7.2#自动关机的最大值最小值
            elif n<6:n=6
            n2=int(n*4096/((1.19/(battery_reference/4096))*2.82))
            while battery_threshold==0 and timeout>0:
                timeout-=1
                uart_send_int(17,n2)
                sleep(0.01)
            if battery_threshold==0:
                raise Exception('错误E15')

    def battery_threshold(self):
        global battery_threshold
        timeout=5
        battery_threshold=0
        while battery_threshold==0 and timeout>0:
            timeout-=1
            uart_send_function(18)
            sleep(0.01)
        if battery_threshold==0:
            raise Exception('错误E16')
        else:
            timeout=5
            while battery_reference==0 and timeout>0:
                timeout-=1
                sleep(1.1)
            if battery_reference!=0:
                return battery_threshold/4096*(1.19/(battery_reference/4096))*2.82
            else:
                raise Exception('错误E19')

    def set_battery_difference_threshold(self,n):
        global battery_difference_threshold
        timeout=5
        battery_difference_threshold=0
        n=int(n*440)
        if n<44:n=44
        elif n>440:n=440
        while battery_difference_threshold==0 and timeout>0:
            timeout-=1
            uart_send_int(19,n)
            sleep(0.01)
        if battery_difference_threshold==0 :
            raise Exception('错误E17')

    def battery_difference_threshold(self):
        global battery_difference_threshold
        timeout=5
        battery_difference_threshold=0
        while battery_difference_threshold==0 and timeout>0:
            timeout-=1
            uart_send_function(20)
            sleep(0.01)
        if battery_difference_threshold==0  :
            raise Exception('错误E18')
        else:
            return battery_difference_threshold/440

    def restart_assistant_MCU(self):
        uart_send_function(21)
        sleep(0.2)

    def get_battery_1(self):
        global battery2,battery_reference
        return battery2/4096*(1.19/(battery_reference/4096))*1.402

    def get_battery_2(self):
        global battery1,battery2,battery_reference
        return (battery1/4096*(1.19/(battery_reference/4096))*2.82)-(battery2/4096*(1.19/(battery_reference/4096))*1.402)

    def get_battery_all(self):
        global battery1,battery_reference
        return battery1/4096*(1.19/(battery_reference/4096))*2.82
#自动显示电池电量图标
#mode0：四格图标，mode1:电压
class battery_display():
    def __init__(self,x=260,y=21,color=(255,255,255),mode=0,erase=False,background_color=(0,0,0)):
        global battery_display_state,battery_x,battery_y,battery_erase,battery_background,battery_color,battery_mode
        battery_display_state=True
        battery_x=x
        battery_y=y
        battery_erase=erase
        battery_background=background_color
        battery_color=color
        battery_mode=mode

    def switch_battery_display(self,state):
        global battery_display_state
        battery_display_state=state


class servo_motor():
    def __init__(self):
        self.vout_on()
        self.channel_on(0)
        self.channel_on(1)
        self.channel_on(2)

    def vout_on(self):
        timeout=5
        while timeout>0 and vout_state!=1:
            timeout-=1
            uart_send_function(7)
            sleep(0.01)
        if vout_state!=1:
            raise Exception('错误E6')

    def vout_off(self):
        timeout=5
        while timeout>0 and vout_state!=0:
            timeout-=1
            uart_send_function(8)
            sleep(0.01)
        if vout_state!=0:
            raise Exception('错误E7')

    def vout_state(self):
        global vout_state
        timeout=5
        vout_state=2
        while timeout>0 and vout_state==2:
            timeout-=1
            uart_send_function(9)
            sleep(0.01)
        if timeout==0:
            raise Exception('错误E8')
        else:
            return bool(vout_state)

    def channel_on(self,n):#打开舵机通道，输入通道号
        global servo1_state,servo2_state,servo3_state
        timeout=5
        if n<0 or n>2 :
            raise Exception('舵机序号错误')
        n=int(n)
        servo1_state=2
        servo2_state=2
        servo3_state=2
        servo_state=2
        while timeout>0 and servo_state!=1:
            timeout-=1
            uart_send_char(10,n+1)
            sleep(0.01)
            if   n==0:servo_state=servo1_state
            elif n==1:servo_state=servo2_state
            elif n==2:servo_state=servo3_state
        if servo_state!=1:
            raise Exception('错误E9')

    def channel_off(self,n):
        global servo1_state,servo2_state,servo3_state
        timeout=5
        if n<0 or n>2 :
            raise Exception('舵机序号错误')
        n=int(n)
        servo1_state=2
        servo2_state=2
        servo3_state=2
        servo_state=2
        while timeout>0 and servo_state!=0:
            timeout-=1
            uart_send_char(11,n+1)
            sleep(0.01)
            if   n==0:servo_state=servo1_state
            elif n==1:servo_state=servo2_state
            elif n==2:servo_state=servo3_state
        if servo_state!=0:
            raise Exception('错误E10')

    def channel_state(self,n):
        global servo1_state,servo2_state,servo3_state
        timeout=5
        if n<0 or n>2 :
            raise Exception('舵机序号错误')
        n=int(n)
        servo1_state=2
        servo2_state=2
        servo3_state=2
        servo_state=2
        while timeout>0 and servo_state==2:
            timeout-=1
            uart_send_char(12,n+1)
            sleep(0.01)
            if   n==0:servo_state=servo1_state
            elif n==1:servo_state=servo2_state
            elif n==2:servo_state=servo3_state
        if servo_state==2:
            raise Exception('错误E11')
        else:
            return bool(servo_state)

    def degree(self,channel,n):
        if channel<0 or channel>2:
            raise Exception('舵机序号错误')
        channel=int(channel)
        if n<0:n=0;
        elif n>180:n=180
        n=180-n
        n=int(n*74+3380)
        uart.writechar(0x47)
        uart.writechar(0x65)
        uart.writechar(13)
        uart.writechar(channel+1)
        uart.writechar(n>>8)#高位
        uart.writechar(int(n%256))#低位
        uart.writechar(13^(channel+1)^(n>>8)^int(n%256))
        sleep(0.002)

    def degree_state(self,channel):
        global angle1,angle2,angle3
        timeout=5
        if channel<0 or channel>2:
            raise Exception('舵机序号错误')
        channel=int(channel)
        angle1=None
        angle2=None
        angle3=None
        angle=None
        while timeout>0 and angle==None:
            timeout-=1
            uart_send_char(14,channel+1)
            sleep(0.005)
            if   channel==0:angle=angle1
            elif channel==1:angle=angle2
            elif channel==2:angle=angle3
        if angle==None:
            raise Exception('错误E12')
        else:
            return 180-((angle-3380)/74)
